package ru.usu.gv.utils.octree;

import javax.vecmath.Point3d;

/**
 * @author Andreas Noack
 * @author spupyrev
 * 05.11.2008
 * 
 * Octtree for graph nodes with positions in 3D space.
 * Contains all graph nodes that are located in a given cuboid in 3D space.
 */
public class Octree
{
	/** Maximum depth of tree nodes. */
	public static final int MAX_DEPTH = 18;
	/** For leafs, the corresponding graph node; for non-leafs <code>null</code>. */
	private IOctreeNode node;
	/** Children of this tree node. */
	private Octree[] children = new Octree[8];
	/** Number of children of this tree node. */
	private int childCount = 0;
	/** Barycenter of the contained graph nodes. */
	private double[] position;
	/** Total weight of the contained graph nodes. */
	private double weight;
	/** Minimum coordinates of the cuboid in each of the 3 dimensions. */
	private double[] minPos;
	/** Maximum coordinates of the cuboid in each of the 3 dimensions. */
	private double[] maxPos;

	/**
	 * Creates an octtree containing exactly one node.
	 */
	public Octree(IOctreeNode node, double[] position, double[] minPos, double[] maxPos)
	{
		this.node = node;
		this.position = new double[] { position[0], position[1], position[2] };
		this.weight = node != null ? node.getWeight() : 0.0;
		this.minPos = minPos;
		this.maxPos = maxPos;
	}

	public IOctreeNode getNode()
	{
		return node;
	}

	public Point3d getPosition()
	{
		return new Point3d(position);
	}

	public int getChildCount()
	{
		return childCount;
	}

	public double getWeight()
	{
		return weight;
	}

	public Octree[] getChildren()
	{
		return children;
	}

	public double[] getMaxPos()
	{
		return maxPos;
	}

	public double[] getMinPos()
	{
		return minPos;
	}

	private double[] constructArray3d(Point3d point)
	{
		return new double[] { point.x, point.y, point.z };
	}

	/**
	 * Adds a graph node to the octtree.
	 */
	public void addNode(IOctreeNode newNode, Point3d newPos)
	{
		addNode(newNode, constructArray3d(newPos), 0);
	}

	private void addNode(IOctreeNode newNode, double[] newPos, int depth)
	{
		if (newNode.getWeight() == 0.0)
			return;

		if (node != null)
		{
			addNode2(node, position, depth);
			node = null;
		}

		for (int d = 0; d < 3; d++)
		{
			position[d] = (weight * position[d] + newNode.getWeight() * newPos[d]) / (weight + newNode.getWeight());
		}
		weight += newNode.getWeight();

		addNode2(newNode, newPos, depth);
	}

	/**
	 * Adds a graph node to the octtree, 
	 * without changing the position and weight of the root.
	 */
	private void addNode2(IOctreeNode newNode, double[] newPos, int depth)
	{
		if (depth == MAX_DEPTH)
		{
			if (children.length == childCount)
			{
				Octree[] oldChildren = children;
				children = new Octree[2 * children.length];
				System.arraycopy(oldChildren, 0, children, 0, oldChildren.length);
			}
			children[childCount++] = new Octree(newNode, newPos, newPos, newPos);
			return;
		}

		int childIndex = 0;
		for (int d = 0; d < 3; d++)
		{
			if (newPos[d] > (minPos[d] + maxPos[d]) / 2)
			{
				childIndex += 1 << d;
			}
		}

		if (children[childIndex] == null)
		{
			double[] newMinPos = new double[3];
			double[] newMaxPos = new double[3];
			for (int d = 0; d < 3; d++)
			{
				if ((childIndex & 1 << d) == 0)
				{
					newMinPos[d] = minPos[d];
					newMaxPos[d] = (minPos[d] + maxPos[d]) / 2;
				}
				else
				{
					newMinPos[d] = (minPos[d] + maxPos[d]) / 2;
					newMaxPos[d] = maxPos[d];
				}
			}

			childCount++;
			children[childIndex] = new Octree(newNode, newPos, newMinPos, newMaxPos);
		}
		else
		{
			children[childIndex].addNode(newNode, newPos, depth + 1);
		}
	}

	/**
	 * Removes a graph node from the octtree. 
	 */
	public void removeNode(IOctreeNode oldNode, Point3d oldPos)
	{
		removeNode(oldNode, constructArray3d(oldPos), 0);
	}

	/**
	 * Removes a graph node from the octtree. 
	 */
	private void removeNode(IOctreeNode oldNode, double[] oldPos, int depth)
	{
		if (oldNode.getWeight() == 0.0)
			return;

		if (weight <= oldNode.getWeight())
		{
			weight = 0.0;
			node = null;
			for (int i = 0; i < children.length; i++)
				children[i] = null;
			childCount = 0;
			return;
		}

		for (int d = 0; d < 3; d++)
		{
			position[d] = (weight * position[d] - oldNode.getWeight() * oldPos[d]) / (weight - oldNode.getWeight());
		}
		weight -= oldNode.getWeight();

		if (depth == MAX_DEPTH)
		{
			int childIndex = 0;
			while (children[childIndex].node != oldNode)
				childIndex++;
			childCount--;
			for (int i = childIndex; i < childCount; i++)
			{
				children[i] = children[i + 1];
			}
			children[childCount] = null;
		}
		else
		{
			int childIndex = 0;
			for (int d = 0; d < 3; d++)
			{
				if (oldPos[d] > (minPos[d] + maxPos[d]) / 2)
				{
					childIndex += 1 << d;
				}
			}
			children[childIndex].removeNode(oldNode, oldPos, depth + 1);
			if (children[childIndex].weight == 0.0)
			{
				children[childIndex] = null;
				childCount--;
			}
		}
	}

	/**
	* Returns the maximum extension of the octtree.
	*/
	public double getWidth()
	{
		double width = 0.0;
		for (int d = 0; d < 3; d++)
		{
			if (maxPos[d] - minPos[d] > width)
			{
				width = maxPos[d] - minPos[d];
			}
		}
		return width;
	}

	/**
	 * Returns the height of the octtree.
	 */
	public int getHeight()
	{
		int height = -1;
		for (Octree child : children)
		{
			if (child != null)
			{
				height = Math.max(height, child.getHeight());
			}
		}
		return height + 1;
	}

	public int getNodeCount()
	{
		if (node != null)
			return 1;

		int result = 0;
		for (Octree child : children)
		{
			if (child != null)
			{
				result += child.getNodeCount();
			}
		}
		return result;
	}
}
